/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include<stdlib.h>
#include<stdio.h>
#include <string>
#include <map>
#include <vector>
#include <list>
#include <sstream>
#include "RangingServer.hpp"
#include "PositionServer.hpp"
#include "owndebug.h"
#include "ownutils.h"
#include "gimutils.h"
#include "TOccupancyGrid.h"

void timer(bool isStart){
	static long int ss1,us1;
	long int ss2,us2,sec,us,ms;
	float fms;
	
	if(isStart) ownTime_GetTimeOfDay(&ss1,&us1);
	else{
		ownTime_GetTimeOfDay(&ss2,&us2);
		sec = ss2-ss1;
		us = us2-us1;
		ms = sec*1000+us/1000;
		fms=(float)sec*1000.0+(float)us/1000.0;
		fprintf(stderr,"Time elapsed (%.6fms = %.6fs) \n",fms,fms/1000.0);
	}
}

/// For reading J2B2 data from filet
int readScanLine181(FILE *f,Grid::TScan &meas, float &x,float &y,float &a){
	float time_s;
	int i;
	char c;
	fscanf(f,"%f %f %f",&x,&y,&a); ///< Pose
	//fprintf(stderr,"1 ");
	for(i=0;i<181;i++){
		fscanf(f,"%f %f",&meas.r[i],&meas.a[i]);
		fscanf(f,"%c",&c);
		// fprintf(stderr,"%.1f ",meas.r[i]);
		if(c=='\n' && i<181){
			//fprintf(stderr,"EOL reached at i=%d\n",i);
			meas.N = i;
			return 0;
		}
	}
	//fprintf(stderr,"3 \n");
	meas.N = 181;
	return 0;
}

int forwardString(const char *buf, int begin,int items){
	int i=begin+1;
	int cnt = 0;
	while(cnt<items){
		if(buf[i]==' ') cnt++;
		i++;
	}
	return i;
}

/// For reading the data from file
int readScanLine(FILE *f,Grid::TScan &meas,Grid::pose &p ){
	float time_s;
	int i=0;
	char in_line[16096];
	char *ret;
	char c=0;
	
	ret = fgets (in_line, 16096, f );
	if(ret==NULL){
		if(feof(f)){
			fprintf(stderr,"Reached end of file -- Alles klar!\n");
			return 0;
		}else{
			fprintf(stderr,"Some error occurred...sorry contact Jari :)\n");
			return 1;
		}
	}
	int cnt=0;
	while( (c!='\n')){
		if(in_line[i]==' ') cnt++;
		else if(in_line[i]=='\n') break;
		i++;
	}
	//fprintf(stderr,"There is %d floats in this file (= %d meas points)\n",cnt,(cnt-3)/2);
	sscanf(in_line,"%f %f %f ",&p.x,&p.y,&p.a); ///< Pose header
	int pointer_ind=0;
	pointer_ind = forwardString(in_line,0,3); ///search the next begin for float
	meas.N = (cnt-3)/2;
	i =0;
	while(i<((cnt-3)/2)){
		sscanf((in_line+pointer_ind),"%f %f",&meas.r[i],&meas.a[i]);
		pointer_ind = forwardString(in_line,pointer_ind,2); ///search the next begin for float
		i++;
	}
	
	return 0;
}


void writeScanLine(FILE *f,Grid::TScan &meas, float x,float y,float a){
	fprintf(f,"%f %f %f ",x,y,a); ///< Pose
	int i;
	for(i=0;i<meas.N;i++){
		fprintf(f,"%f %f ",meas.r[i],meas.a[i]);
	}
	fprintf(f,"%f %f\n",meas.r[i],meas.a[i]);
}
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
void showUsage(){
	fprintf(stderr,"tilted laser data manipulator\n");
	fprintf(stderr,"./avant_laser_old_format -P [Logfile]\n");
}
/********************************************************************************/
/********************************************************************************/
///Compute 3D points
/********************************************************************************/
/********************************************************************************/
struct p3f{
	float x;
	float y;
	float z;
	
	p3f():x(0),y(0),z(0){}
	p3f(float _x,float _y, float _z):x(_x),y(_y),z(_z){}
	void set(float _x,float _y, float _z){ x=_x;y=_y;z=_z;}
};


void compute3DProjection(Grid::TScan &meas,Grid::pose &pose, std::vector<p3f> &points){
	float L1 = 1.750; ///Offset between laser and the front axis
	float laserHeight = 0;
	float laserOffset = M_PI; ///the last one is an error in logfile
	float x,y;
	
	
	///Laser position
	x = pose.x - L1 * cos(pose.a);
	y = pose.y - L1 * sin(pose.a);
	
	
	float ctheta = cos((17.0)*M_PI/180.0);
	float stheta =  sin((17.0)*M_PI/180.0);
	float chead = cos(pose.a);
	float shead = sin(pose.a);
	
	
	for(int i=0;i<meas.N;i++){ ///For each measurement point
		
	
		//if(r_meas->distance > RangingMinDistance && r_meas->distance<RangingMaxDistance){
			if(meas.r[i] > 0.1 && meas.r[i] < 20){
			//fprintf(stderr,"%.2f ",r_meas->angle);
			float xsw = meas.r[i] * cos(meas.a[i]+laserOffset) * ctheta;
			float ysw = meas.r[i] * sin(meas.a[i]+laserOffset);
			float zsw = meas.r[i] * cos(meas.a[i]+laserOffset) * stheta;
			
			p3f point;
			point.x = xsw * chead - ysw * shead + x;
			point.y = xsw * shead + ysw*chead + y;
			point.z =laserHeight - zsw;
			points.push_back(point);
			//fprintf(stderr,"(%.2f,%.2f,%.2f) (%.3f,%.3f,%.3f) %3f\n",x,y,pose->a, point.x,point.y,point.z,r_meas->distance);
		}
	}
	//fprintf(stderr,"--------------------------------------------------------------------------\n");
}





/********************************************************************************/
/********************************************************************************/
///Execute
/********************************************************************************/
/********************************************************************************/
void execute(std::string logfilename){
	int rangingCounter=0;
	int positionCounter = 0;
	std::vector<p3f> points;
	
	Grid::pose pose_now,last,current,dP;
	int cnt=0;
	
	FILE *logfile = fopen(logfilename.c_str(),"rt");
	Grid::TScan meas;
	meas.alloc(1500);
	
	while(!feof(logfile)){
		readScanLine(logfile,meas, current);
		
		
		if(cnt!=0){
			dP.setToDifferentialPose(current,last);
			pose_now = pose_now.integrateDifferential(dP);
		}
		
		cnt++;
		last=current;
		compute3DProjection(meas,pose_now,points);
	}
	
	
	TOccupancyGrid O(120.0, 120.0,0.2,Grid::TCoord2D(0,0));
	
	
	
	///SAVE
	FILE *f;
	f= fopen("data.txt","wt");
	for(int i=0;i<points.size();i++){
		fprintf(f,"%f %f %f\n",points[i].x,points[i].y,-points[i].z);
	}
	fclose(f);

}







/********************************************************************************/
/********************************************************************************/
/// Main
/********************************************************************************/
/********************************************************************************/

int main(int argc, char *argv[]){
	// Init Debug lib
	int verbose = 5;
	debugInit();
	debugSetGlobalDebugLvl(verbose);
	debugSetLogFilename("tilted_laser.log");
	std::string LogFile;

	
	
	bool parse = true;
	while(parse) {
		int c;
		c = getopt(argc, argv, "P:R:hqv");
		switch(c) {
			
			/////////////////////////////////
			////// Application specific /////
			/////////////////////////////////
			case 'P': {
					LogFile = optarg;
				break;
			}
			
				//////////////////////////////////
				//// GENERIC:                 ////
				//////////////////////////////////
			case 'v':
				// Increase only if not previously set to 0 (by 'quiet')
				if (verbose > 0)
					debugSetGlobalDebugLvl(++verbose);
				break;
				
			case 'q':
				verbose = 0;
				debugSetGlobalDebugLvl(0);
				break;
				
			case 'h':
			case '?':
				showUsage();
				exit(1);
				break;
				
			case -1:
				parse = false;
				break;
		}
	}
	
	
	
	execute(LogFile);
	
	
	
	return 0;
}
